#include <unistd.h>
#include <sys/mman.h>
#include <string.h>
#include <stdio.h>
#include "deviser.h"
#include "gc.h"

#include "gc_macros.h"

unsigned int num_cons = 0;
unsigned int used_cells = 0;
unsigned int num_flips = 0;
unsigned int alloced_cells = 0;
int halted = -1;

unsigned int symbolp(cell* sym);

ext_ref * root_set = NULL;

ext_ref eol_ref, car_ref, cdr_ref, symbols, true_ref, false_ref;

cell mem_start = NULL;
cell mem_end   = NULL;
cell mem_free  = NULL;

cell mk_eol()
{
	return eol_ref.ref;
}

void forward(cell* _ref)
{
//	fprintf(stderr, "forward\n");
	cell* ref = _ref;
	do {
//		fprintf(stderr, "ref = %p\n", ref);
		ref = (cell*) untag(*ref);
	} while( get_tag(*ref) == CDR_FORW );
	*_ref = tag(ref, get_tag(*_ref));
}

/*unsigned int forwardp(cell ref) {
	return get_tag(*((cell *) ref)) == CDR_FORW;
	}*/

unsigned int consp(cell* ref)
{
	forward(ref);
	return get_tag(*untag(*ref)) != CDR_NOT_CONS;
}

cell ecar(cell ref) {
	return car(&ref);
}

cell ecdr(cell ref) {
	return cdr(&ref);
}

cell car(cell* ref)
{
	if(!consp(ref)) return dvs_error("Type mismatch: expected cons");
	return untag(*untag(*ref));
}

cell cdr(cell* ref)
{
	if(!consp(ref)) return dvs_error("Type mismatch: expected cons");
	switch(get_tag(*untag(*ref)))
	{
		case CDR_NORMAL  : return (cell) *untag(*ref + 1);
		case CDR_NEXT    : return untag(*ref + 1);
	}
	return 0;
}

unsigned int gc_copy(cell* ref)
{
	unsigned int cells_allocated = 0;
	int fix_ref=1;
	int list_iter = 0;
	cell _ref;
	cell new_loc;
	_ref = *ref;
	forward(ref);
gc_copy_top:
	new_loc = mem_free;
	forward(&_ref);

	/* If the object is already in new memory, skip it. */
	if(untag(_ref) >= mem_start && untag(_ref) < mem_free) return 0;
//	fprintf(stderr, "Copying: ");
//	fflush(stderr);
	if(consp(&_ref))
	{
//		fprintf(stderr, "cons (%p), ", _ref);
//		fflush(stderr);
		/* Cell is the head of a pair. */
		cell _cdr, _car;
		_car = car(&_ref);
//		fprintf(stderr, "(car %p: ",_car);
//		fflush(stderr);
//		fprintf(stderr, "%p)", *untag(_car));
//		fflush(stderr);
		forward(&_car);
		_cdr = cdr(&_ref); forward(&_cdr);
		if(get_tag(*untag(_ref))==CDR_NORMAL)
			*(untag(_ref)+1) = multicell_head(0, EMPTY);

		if(1)// || _cdr >= mem_start && _cdr < mem_free)
		{
//			fprintf(stderr, "normal\n");
//			fflush(stderr);
			/* The cdr is already in new memory, copy as a normal pair */
			*mem_free++ = _car;
			*mem_free++ = _cdr;
			cells_allocated += 2;
		}
		else
		{
//			fprintf(stderr, "compact\n");
//			fflush(stderr);
			/* This can be a compacted pair, copy its cdr to new memory now. */
			*mem_free++ = tag(_car, CDR_NEXT);
			/* Leave a forwarding address. (usually done at the end of this function. */
			*untag(_ref) = tag(new_loc, CDR_FORW);
			if(fix_ref) *ref = tag(new_loc, get_tag(_ref));
			/* Copy the CDR. */
			_ref=_cdr;
			fix_ref=0;
			cells_allocated +=1;
			list_iter ++;
			goto gc_copy_top;
		}
	}
	else 
	{
//		fprintf(stderr, "notcons\n");
//		fflush(stderr);
		/* Cell is a NOTCONS. */
		*mem_free++ = *untag(_ref);
		cells_allocated += 1;
		if(is_multicell(*untag(_ref)))
		{
			/* If this is a multi-cell object, copy it all into new memory. */
			unsigned int size;
			cell cur;
			size = get_data(*untag(_ref));
			cur = untag(_ref) + 1;
			memcpy(mem_free, cur, size*sizeof(cell));
			mem_free += size;
			if(size > 0)
			{
				/* Leave behind an EMPTY object to take up the rest of the space vacated. */
				*(untag(_ref)+1) = multicell_head(size-1, EMPTY);
			}
			cells_allocated += size;
		}
	}
	/* Leave a forwarding address. */
	*untag(_ref) = tag(new_loc, CDR_FORW);
	if(fix_ref)	*ref = tag(new_loc, get_tag(_ref));
	return cells_allocated;
}	

unsigned int copy_root_set()
{
	unsigned int rtn=0;
	ext_ref * cur = root_set;
	while(cur)
	{
//		fprintf(stderr, "%p %p...", cur, cur->ref);
//		fflush(stderr);
		if(cur->ref) rtn += gc_copy(&(cur->ref));
//		fprintf(stderr, "copied (%p)\n", cur->ref);
//		fflush(stderr);
		
		cur = cur->next;
	}
	return rtn;
}

unsigned int copy_refs(cell val)
{
	unsigned int rtn;
//	fprintf(stderr, "copy_refs(%p)... ", val);
//	fflush(stderr);
	if(consp(&val))
	{
//		fprintf(stderr, "CAR... ");
//		fflush(stderr);
		rtn = gc_copy((cell*)val); /* car */
		
//		fprintf(stderr, "CDR...");
//		fflush(stderr);
		if( get_tag(*val) == CDR_NORMAL ) rtn += gc_copy((cell*)val+1); /* cdr */
		return rtn;
	}
	if(symbolp(&val))
	{
		return gc_copy((cell*)(val+1));
	}
	if(lfuncp(&val))
	{
		return gc_copy((cell*)(val+1))
		     + gc_copy((cell*)(val+2));

	}
//	fprintf(stderr, "\n");
	return 0;
}


void gc(unsigned int num)
{
	unsigned int size;
	cell old_start, old_end;
	cell cur;
	fprintf(stderr, "\nRunning GC...\n");
	fflush(stderr);
	old_start = mem_start;
	old_end = mem_free;
	size = ((unsigned int)mem_end - (unsigned int)mem_start) + getpagesize();
	fprintf(stderr, "  size = %u\n", size);
	fprintf(stderr, "  %d used cells\n", mem_free-mem_start);
	mem_start = (cell) mmap(0, size, PROT_READ | PROT_WRITE, MAP_PRIVATE | MAP_ANONYMOUS,0,0);
	fprintf(stderr, "  mem_start = %p\n", mem_start);
	fflush(stderr);
	mem_end = mem_start + size/sizeof(cell*);
	mem_free = mem_start;

	fprintf(stderr, "  root_set = %p\n", root_set);
	fflush(stderr);
	copy_root_set();
	fprintf(stderr, "  root set copied.\n");
	fflush(stderr);
	cur = mem_start;
	while(cur < mem_free)
	{
		copy_refs(cur);
		switch(get_tag(*cur))
		{
			case CDR_NORMAL:
				cur += 2; break;
			case CDR_NEXT:
				cur ++; break;
			case CDR_NOT_CONS:
				if (is_multicell(*cur))
				{
					cur += 1 + get_data(*cur);
				}
				else cur++;
				break;
			case CDR_FORW:
				cur ++; break;
		}
	}
	fprintf(stderr, "  unmapping...\n");
	fflush(stderr);
	munmap(old_start, size-getpagesize());
	while((unsigned int)mem_end - (unsigned int)(mem_free + num) > 3*getpagesize()/2)
	{
		mem_end -= getpagesize()/sizeof(cell);
		munmap(mem_end, getpagesize());
	}
	fprintf(stderr, "done:");
	fprintf(stderr, "  %d used cells\n", mem_free-mem_start);
	fflush(stderr);
}

cell old_start = NULL, old_end = NULL;
cell gc_sweep = NULL;
unsigned int progress_ratio = 4;

void gc_start();

void gc_stop(unsigned int n)
{
	gc_sweep = NULL;
	used_cells = mem_free - mem_start;
}

void gc_start()
{
	unsigned int size;
	unsigned int old_size;

//	fprintf(stderr, "gc_start()\n");
	if(gc_sweep)
	{
		fprintf(stderr, "GC already started!\n");
		return;
	}
	alloced_cells = 0;
	size = 1 + (unsigned int)(((unsigned int)mem_free - (unsigned int)mem_start)*(1.0+1.0/(float)progress_ratio));
	size /= getpagesize();
	size += 2;
//	fprintf(stderr, "GC: size = %u pages\n", size);
	size *= getpagesize();
	
	old_size = (unsigned int)old_end - (unsigned int)old_start;
	if(old_size < size)
	{
		unsigned int new_size;
		new_size = getpagesize();
		while(new_size < size) new_size *= 2;
		
		fprintf(stderr, "GC: size = %u -> %u\n", old_size, new_size);
		fprintf(stderr, "Freeing old memory: %p -> %p\n", old_start, old_end);
		munmap(old_start, old_size);
		old_start = mem_start;
		old_end = mem_end;
		
		mem_start = (cell) mmap(0, new_size, PROT_READ | PROT_WRITE, MAP_PRIVATE | MAP_ANONYMOUS,0,0);
		mem_end = mem_start + new_size/sizeof(cell*);
		fprintf(stderr, "Allocated new memory: %p -> %p\n", mem_start, mem_end);
		num_flips = 0;
	}
	else if(num_flips > 15 && old_size > 4 * size)
	{
		cell st, en;
		fprintf(stderr, "GC: size = %u -> %u\n", old_size, old_size / 2);
	
		fprintf(stderr, "Freeing extra memory: %p -> %p\n", old_start + (old_size / sizeof(cell)) / 2, old_end);
		munmap(old_start + (old_size / sizeof(cell)) / 2, old_size / 2);

		old_end = old_start + (old_size / sizeof(cell)) / 2;
			
		st = old_start;
		en = old_end;
			
		old_start = mem_start;
		old_end   = mem_end;
				
		mem_start = st;
		mem_end   = en;
		num_flips = 0;
	}
	else
	{
		cell st, en;
		
		st = old_start;
		en = old_end;
		
		old_start = mem_start;
		old_end   = mem_end;
			
		mem_start = st;
		mem_end   = en;
		if(old_size > 4*size) num_flips ++;
		else(num_flips = 0);
	}
	mem_free = mem_start;

	copy_root_set();

	gc_sweep = mem_start;
}

void gc_finish()
{
	while(gc_sweep < mem_free)
	{
		copy_refs(gc_sweep);
		switch(get_tag(*gc_sweep))
		{
			case CDR_NORMAL:
				gc_sweep += 2; break;
			case CDR_NEXT:
				gc_sweep ++; break;
			case CDR_NOT_CONS:
				if (is_multicell(*gc_sweep))
				{
					gc_sweep += 1 + get_data(*gc_sweep);
				}
				else gc_sweep++;
				break;
			case CDR_FORW:
				gc_sweep ++; break;
		}
	}
	gc_stop(0);
}

void gc_step(unsigned int n)
{
	cell target;
	if(halted >= 0){ halted += n; return; }
	target = gc_sweep + (1 + progress_ratio) * n;
	if(gc_sweep)
	{
		while(gc_sweep < target)
		{
			if(gc_sweep >= mem_free)
			{
				unsigned int k = copy_root_set();
				if(!k)
				{
					gc_stop(n);
					return;
				}
//				target += k;
			}
//			target += progress_ratio * 
			copy_refs(gc_sweep);
			switch(get_tag(*gc_sweep))
			{
				case CDR_NORMAL:
					gc_sweep += 2; break;
				case CDR_NEXT:
					gc_sweep ++; break;
				case CDR_NOT_CONS:
					if (is_multicell(*gc_sweep))
					{
						gc_sweep += 1 + get_data(*gc_sweep);
					}
					else gc_sweep++;
					break;
				case CDR_FORW:
					gc_sweep ++; break;
			}
		}
	}
	else
	{
//		fprintf(stderr, "%p %d %p\n", mem_free, n, mem_end);
//		if(mem_free + n - mem_start >= (progress_ratio +2) * (mem_end - (mem_free + n)) / 2) gc_start();
//		if(mem_free + n - mem_start >=
//		   (progress_ratio + 2) * (mem_start + (mem_end - mem_start)/2 - (mem_free + n)))
			gc_start();
	}
}

cell alloc_cell(unsigned int num)
{
	cell rtn;
//	fflush(stderr);
	gc_step(num);
//	if( mem_free + num >= mem_end ) gc(num);
	rtn = mem_free;
	mem_free += num;
	alloced_cells += num;
	return rtn;
}

void init_gc()
{
	mem_start = (cell) mmap(0, getpagesize(), PROT_READ | PROT_WRITE, MAP_PRIVATE | MAP_ANONYMOUS,0,0);
	mem_free = mem_start;
	mem_end = mem_start + (getpagesize() / sizeof(cell));
//	fprintf(stderr, "Allocated initial memory: %p -> %p\n", mem_start, mem_end);
		
	old_start = old_end = NULL;
	
	root_set = &eol_ref;
	eol_ref.next = &car_ref;
	eol_ref.ref = mem_free++;
	*(eol_ref.ref) = (cell)EOL;
	car_ref.next = &cdr_ref;
	cdr_ref.next = &symbols;
	symbols.next = &true_ref;
	true_ref.next = &false_ref;
	false_ref.next = NULL;
	
	symbols.ref = NULL;
	car_ref.ref = NULL;
	cdr_ref.ref = NULL;
	true_ref.ref = mem_free++;
	*(true_ref.ref) = (cell)(BOOL | (0xffff0000));
	false_ref.ref = mem_free++;
	*(false_ref.ref) = (cell)BOOL;

	symbols.ref = mk_eol();

	copy_root_set();
	gc_sweep = mem_start;
}

unsigned int enullp(cell ref) {
	return nullp(&ref);
}

unsigned int nullp(cell* ref)
{
	forward(ref);
	return *ref == eol_ref.ref;
}

unsigned int numberp(cell* ref)
{
	forward(ref);
	return get_type(*untag(*ref)) == SHORT;
}

cell cons(cell _car, cell _cdr)
{
	num_cons++;
	car_ref.ref = _car;
	cdr_ref.ref = _cdr;
	cell rtn = alloc_cell(2);
	rtn[0] = car_ref.ref;
	rtn[1] = cdr_ref.ref;
	car_ref.ref = NULL;
	cdr_ref.ref = NULL;
	return rtn;
}

unsigned short number_to_cint(cell ref)
{
	if(get_type(*ref) == SHORT) return get_data(*ref);
	return ~0;
//	return get_data(*ref);
}

cell mk_number(unsigned short n)
{
	cell rtn = alloc_cell(1);
	*rtn = (void *)(((unsigned int) n) << 16 | SHORT);
	return rtn;
}


cell mk_string(const char *data, unsigned int len)
{
	int words = 3+len/sizeof(cell); /* FIXME: occasionally allocates an extra cell */
	cell rtn = alloc_cell(words);
//	fprintf(stderr, "%d\n", words);
	memset(rtn,0, words * sizeof(cell));
	rtn[0] = multicell_head(words - 1, STRING);
//	fprintf(stderr, "%p\n", rtn[0]);
	rtn[1] = (cell)(len);
	memcpy(rtn + 2, data, len);
	return rtn;
}

unsigned int stringp(cell* ref)
{
	forward(ref);
	return get_type(*untag(*ref)) == STRING;
}

unsigned int get_str_len(cell* ref)
{
	cell str;
	if(!stringp(ref)) return 0;
	str = untag(*ref);
	return (unsigned int) str[1];
}

const char * get_str_val(cell* ref)
{
	cell str;
	if(!stringp(ref)) return 0;
	str = untag(*ref);
	return (const char *) (str + 2);
}

unsigned int eeqp(cell ref1, cell ref2) {
	return eqp(&ref1, &ref2);
}

unsigned int eqp(cell* ref1, cell* ref2)
{
	forward(ref1);
	forward(ref2);
	return untag(*ref1) == untag(*ref2);
}

unsigned int eeqvp(cell ref1, cell ref2) {
	return eqvp(&ref1, &ref2);
}

unsigned int eqvp(cell* ref1, cell* ref2)
{
	if(eqp(ref1, ref2)) return 1;
	if( consp(ref1) || consp(ref2) )
	{
		if( consp(ref1) && consp(ref2) )
			return (car(ref1) == car(ref2)) && (cdr(ref1) == cdr(ref2));
		else return 0;
	}
	
	if( is_multicell(*untag(*ref1)) || is_multicell(*untag(*ref2)))
	{
		if( is_multicell(*untag(*ref1)) && is_multicell(*untag(*ref2)))
		{
			if(get_data(*untag(*ref1)) != get_data(*untag(*ref2))) return 0;
			return (memcmp(untag(*ref1), untag(*ref2), (1+get_data(*untag(*ref1)))*sizeof(cell))) == 0;
		}
		else return 0;
	}
	return *untag(*ref1) == *untag(*ref2);
}

unsigned int symbolp(cell* sym)
{
	forward(sym);
	return get_type(*untag(*sym)) == SYMBOL;
}

cell symbol_to_string(cell* sym)
{
	if(!symbolp(sym)) return NULL;
	return *(1+untag(*sym));
}

cell mk_gensym(const char * data, unsigned int len)
{
	ext_ref str;
	ext_ref list;
	ext_ref rtn;

	str.ref = NULL;
	list.ref = NULL;
	rtn.ref = NULL;
	str.next = &list;
	list.next = &rtn;
	rtn.next = root_set;
	root_set = &str;

	str.ref = mk_string(data, len);
	
	/* New symbol */

	rtn.ref = alloc_cell(2);
	rtn.ref[0] = multicell_head(1, SYMBOL);
	rtn.ref[1] = str.ref;
	
	root_set = rtn.next;
	return rtn.ref;
}

cell mk_symbol(const char * data, unsigned int len)
{
	ext_ref str;
	ext_ref list;
	ext_ref rtn;

	str.ref = NULL;
	list.ref = NULL;
	rtn.ref = NULL;
	str.next = &list;
	list.next = &rtn;
	rtn.next = root_set;
	root_set = &str;

	str.ref = mk_string(data, len);
	list.ref = symbols.ref;
	while (!nullp(&(list.ref)))
	{
		cell c1, c2;
		c1 = car(&(list.ref));
		c2 = symbol_to_string(&c1);
		if(eqvp(&c2, &(str.ref)))
		{
			rtn.ref = c1;
			root_set = rtn.next;
			return rtn.ref;
		}
		list.ref = cdr(&(list.ref));
	}
	
	/* New symbol */

	rtn.ref = alloc_cell(2);
	rtn.ref[0] = multicell_head(1, SYMBOL);
	rtn.ref[1] = str.ref;
	
	symbols.ref = cons(rtn.ref, symbols.ref);
	root_set = rtn.next;
	return rtn.ref;
}

void set_cdr(cell pair, cell new_cdr) {
	forward(&pair);
	forward(&new_cdr);

	if(!consp(&pair)) /*return*/ dvs_error("set_cdr with a non-cons");
	gc_copy(&new_cdr);
	if(get_tag(pair) == CDR_NEXT) 
		*pair = tag(cons(car(&pair), new_cdr), CDR_FORW);
	else if(get_tag(pair) == CDR_NORMAL) {
		cell* p = (cell*)pair;
		p[1] = new_cdr;
	}
}

cell mk_cfunc(dvs_func f)
{
	cell rtn;
	rtn = alloc_cell(2);
	rtn[0] = multicell_head(1, CFUNC);
	rtn[1] = f;
	return rtn;
}

unsigned int cfuncp(cell* val)
{
	forward(val);
	return get_type(*untag(*val)) == CFUNC;
}

dvs_func get_cfunc(cell* val)
{
	forward(val);
	if(!cfuncp(val)) return (dvs_func) dvs_error("Type Mismatch: expected CFUNC");
	return (dvs_func)((*val)[1]);
}

cell mk_lfunc(cell* env, cell* code)
{
	forward(env);
	forward(code);
	cell rtn;
	car_ref.ref = *env;
	cdr_ref.ref = *code;
	rtn = alloc_cell(3);
	rtn[0] = multicell_head(2, LFUNC);
	rtn[1] = car_ref.ref;
	rtn[2] = cdr_ref.ref;
	car_ref.ref = NULL;
	cdr_ref.ref = NULL;
	return rtn;
}

unsigned int lfuncp(cell* val)
{
	forward(val);
	return get_type(*untag(*val)) == LFUNC;
}

cell get_env(cell* fun)
{
	if(!lfuncp(fun)) return dvs_error("Type Mismatch: expected LFUNC");
	return (*fun)[1];
}

cell get_code(cell* fun)
{
	if(!lfuncp(fun)) return dvs_error("Type Mismatch: expected LFUNC");
	return (*fun)[2];
}

cell mk_bool(unsigned int val)
{
	if(val) return (true_ref.ref);
	return (false_ref.ref);
}

unsigned int boolp(cell* val)
{
	forward(val);
	return get_type(*untag(*val)) == BOOL;
}

unsigned int falsep(cell* val)
{
	forward(val);
	return *val == false_ref.ref;
}

unsigned int gc_halt()
{
	unsigned int rtn;
	fprintf(stderr, "GC Halted!\n");
	rtn = mem_end - mem_start - alloced_cells - used_cells;
	if(halted < 0) halted = 0;
	return rtn;
}

void gc_resume()
{
	unsigned int n = halted;
	fprintf(stderr, "GC Resumed\n");
	if(n < 0) return;
	halted = -1;
	gc_step(n);
}

